static int open_loop = 0;                  // 是否开环控制，默认为否
static double open_loop_duty_cycle = 50.0; // 开环控制的输出
static double Kp = 0.197282666954605;
static double Ki = 3.63388243633273;
static double Kd = 0.00267760799658669;
static double target_speed = 220.0;

static double integral = 0.0;
static double derivative = 0.0;
static double error_last = 0.0;

static int sample_period_us;

// 根据当前速度，计算出需要输出的PWM占空比:百分之output
__attribute__((export_name("pid_controller"))) double pid_controller(double current_speed)
{
    // 开环：直接输出设定的占空比。闭环：根据转速，计算输出占空比
    if (open_loop)
        return open_loop_duty_cycle;
    double error = target_speed - current_speed;
    integral += error;
    derivative = error - error_last;
    double output = Kp * error + Ki * (sample_period_us / 1000.0 / 1000.0) * integral + Kd / (sample_period_us / 1000.0 / 1000.0) * derivative;
    error_last = error;
    if (output < 0.0)
        output = 0.0;
    else if (output > 100.0)
        output = 100.0;
    return output;
}

__attribute__((export_name("set_sample_period_us"))) void set_sample_period_us(int period_us)
{
    sample_period_us = period_us;
}

// 重置积分，微分，误差
__attribute__((export_name("reset_controller_state"))) void reset_controller_state()
{
    derivative = 0.0;
    integral = 0.0;
    error_last = 0.0;
}

__attribute__((export_name("set_open_loop"))) void set_open_loop(int ol)
{
    open_loop = ol;
}

__attribute__((export_name("get_open_loop"))) int get_open_loop()
{
    return open_loop;
}

__attribute__((export_name("set_open_loop_duty_cycle"))) void set_open_loop_duty_cycle(double oldc)
{
    open_loop_duty_cycle = oldc;
}

__attribute__((export_name("get_open_loop_duty_cycle"))) double get_open_loop_duty_cycle()
{
    return open_loop_duty_cycle;
}

__attribute__((export_name("set_Kp"))) void set_Kp(double k_p)
{
    Kp = k_p;
}

__attribute__((export_name("get_Kp"))) double get_Kp()
{
    return Kp;
}

__attribute__((export_name("set_Ki"))) void set_Ki(double k_i)
{
    Ki = k_i;
}

__attribute__((export_name("get_Ki"))) double get_Ki()
{
    return Ki;
}

__attribute__((export_name("set_Kd"))) void set_Kd(double k_d)
{
    Kd = k_d;
}

__attribute__((export_name("get_Kd"))) double get_Kd()
{
    return Kd;
}

// 设置目标速度
__attribute__((export_name("set_target_speed"))) void set_target_speed(double speed_rpm)
{
    if (speed_rpm < 0.0)
    {
        target_speed = 0.0;
        return;
    }
    if (speed_rpm > 320)
    {
        target_speed = 320.0;
        return;
    }
    target_speed = speed_rpm;
}

__attribute__((export_name("get_target_speed"))) double get_target_speed()
{
    return target_speed;
}
